#include "sdk.h"
#include "debug.h"
#include "elf.h"
#include "eloader.h"
#include "malloc.h"

int relocate_entry2(tRelEntry* reloc_entry, void* reloc_addr, int iLoop, int relocsCount)
{
	u32 buffer = 0, code = 0, offset = 0, offset_target;

	// Actual offset
	offset_target = (u32)reloc_entry->r_offset + (u32)reloc_addr;

	// Load word to be relocated into buffer
	u32 i;
	u32 misalign = offset_target % 4;
    if (misalign) 
	{
        u32 array[2];
        array[0] = *(u32*)(offset_target - misalign);
        array[1] = *(u32*)(offset_target + 4 - misalign);
        u8* array8 = (u8*)&array;
        u8* buffer8 = (u8*)&buffer;
        for (i = 3 + misalign; i >= misalign; --i)
		{
            buffer8[i-misalign] = array8[i];
        }
    } 
	else 
	{
        buffer = *(u32*)offset_target;
    }

	int type = reloc_entry->r_info & 0x000000FF;
//	int symbol = reloc_entry.r_info >> 8;
//	int iOfsPH = symbol & 0xFF;
//	int iValPH = (symbol >> 8) & 0xFF;

//	u32 dwRealOfs = reloc_entry->r_offset + m_pElfPrograms[iOfsPH].iVaddr;
//	u32 dwCurrBase = dwBase + m_pElfPrograms[iValPH].iVaddr;
	u32 dwRealOfs = (u32)reloc_entry->r_offset;
	u32 dwCurrBase = 0;
	
	//Relocate depending on reloc type
	switch(type)
	{
		// Nothing
		case R_MIPS_NONE:
			return 0;
			
		// 32-bit address
		case R_MIPS_32:
			buffer += (u32)reloc_addr;
			break;
		
		// Jump instruction
		case R_MIPS_26:
			code = buffer & 0xfc000000;
			offset = (buffer & 0x03ffffff) << 2;
			offset += (u32)reloc_addr;
			buffer = ((offset >> 2) & 0x03ffffff) | code;
			break;
		
		// Low 16 bits relocate as high 16 bits
		case R_MIPS_HI16:
/*
			if((u32)reloc_entry.r_offset == 0x000004D8){
				LOGSTR0("!!==> 0x000004D8 :\n");
				LOGSTR1("!!====> opcode: 0x%08lX\n", buffer);
				LOGSTR1("!!====> loc addr: 0x%08lX\n", ((buffer & 0x0000FFFF)<<16));
			}
			offset = ((buffer & 0x0000FFFF)<<16) + (u32)reloc_addr;
			// WHY ??? Maybe because of sh_type which is 0x700000A0 ?
			if(offset & 0x000F0000){
				offset += 0x00010000;
			}
			offset = offset >> 16;
			if((u32)reloc_entry.r_offset == 0x000004D8){
				LOGSTR1("!!====> real addr: 0x%08lX\n", offset);
			}
			buffer = (buffer & 0xffff0000) | offset;
			break;
*/
			{
				u32 inst;
				int base = iLoop;
				int lowaddr, hiaddr, addr;
			  	int loinst;
			//	int ofsph = m_pElfPrograms[iOfsPH].iVaddr;
				tRelEntry* next_entry = reloc_entry;

				addr = ((buffer & 0xFFFF) << 16) + dwCurrBase;
				LOGSTR2("Hi at (%08lX) %d\n", dwRealOfs, iLoop);
			  	while(++iLoop < relocsCount){
					next_entry++;
			  		if((next_entry->r_info & 0x000000FF) != R_MIPS_HI16)break;
			  	}
				LOGSTR1("Matching low at %d\n", iLoop);
			  	if (iLoop < relocsCount) {
					LOGSTR3("loinst = *(u32*)([0x%08lX->r_offset[0x%08lX] + (u32)0x%08lX\n", (u32)next_entry, (u32)next_entry->r_offset, (u32)reloc_addr);
					LOGSTR1("loinst_addr: 0x%08lX\n", (u32)(((u32)next_entry->r_offset) + ((u32)reloc_addr)));
					loinst = ((u32*)(((u32)next_entry->r_offset) + ((u32)reloc_addr)))[0];
					LOGSTR1("loinst ==> 0x%08lX\n", loinst);
				}else{
					loinst = 0;
				}
				LOGSTR0(" a\n");
				addr = (s32)addr + (s16)(loinst & 0xFFFF);
				LOGSTR0(" b\n");
				lowaddr = addr & 0xFFFF;
				LOGSTR0(" c\n");
				hiaddr = (((addr >> 15) + 1) >> 1) & 0xFFFF;
				LOGSTR0(" d\n");
				LOGSTR3("addr: 0x%08lX (0x%08lX + 0x%08lX)\n", addr, hiaddr, lowaddr);

				next_entry = reloc_entry;
				while(base < iLoop){
					inst = *(u32*)(next_entry->r_offset + (u32)reloc_addr);
					inst = (inst & ~0xFFFF) | hiaddr;
					*(u32*)(next_entry->r_offset + (u32)reloc_addr) = inst;
					base++;
					next_entry++;
				}
				LOGSTR0(" e\n");

			  	while(iLoop < relocsCount){
					inst = *(u32*)(next_entry->r_offset + (u32)reloc_addr);
					if ((inst & 0xFFFF)!=(loinst & 0xFFFF))break;
					inst = (inst & ~0xFFFF) | lowaddr;
					*(u32*)(next_entry->r_offset + (u32)reloc_addr) = inst;

					iLoop++;
					next_entry++;
			  		if((next_entry->r_info & 0x000000FF) != R_MIPS_LO16)break;
				}
				iLoop--;
				LOGSTR1("Finished at %d\n", iLoop);
			}


		
		// Low 16 bits relocate as low 16 bits
		case R_MIPS_LO16:
			offset = (buffer & 0x0000ffff) + (u32)reloc_addr;
			buffer = (buffer & 0xffff0000) | (offset & 0x0000ffff);
			break;
		
		default:
			return -1;		
	}
	
	// Restore relocated word
    if (misalign) 
	{
        u32 array[2];
        array[0] = *(u32*)(offset_target - misalign);
        array[1] = *(u32*)(offset_target + 4 - misalign);
        u8* array8 = (u8*)&array;
        u8* buffer8 = (u8*)&buffer;
        for (i = 3 + misalign; i >= misalign; --i)
		{
             array8[i] = buffer8[i-misalign];
        }
        *(u32*)(offset_target - misalign) = array[0];
        *(u32*)(offset_target + 4 - misalign) = array[1];
    } 
	else 
	{ 
        *(u32*)offset_target = buffer;
    }

	return 0;
}
